#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time    : 2019/1/24 14:19
# @Author  : Seven
# @File    : L-KImage.py
# @Software: PyCharm
import cv2
import numpy as np

# 加载视频
cap = cv2.VideoCapture()
cap.open('768x576.avi')
if not cap.isOpened():
    print("无法打开视频文件")

# ShiTomasi 角点检测参数
feature_params = dict(maxCorners=100,
                      qualityLevel=0.3,
                      minDistance=7,
                      blockSize=7)

# lucas kanade光流法参数
# maxlevel 是图像金字塔的层数
lk_params = dict(winSize=(15, 15),
                 maxLevel=2,
                 criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))

# 创建随机颜色
color = np.random.randint(0, 255, (100, 3))

# 获取第一帧，找到角点
ret, old_frame = cap.read()
# 找到原始灰度图
old_gray = cv2.cvtColor(old_frame, cv2.COLOR_BGR2GRAY)
old_gray = cv2.GaussianBlur(old_gray, (5, 5), 0)
# 获取图像中的角点，返回到p0中
p0 = cv2.goodFeaturesToTrack(old_gray, mask=None, **feature_params)

# 创建一个模板用来画轨迹
mask = np.zeros_like(old_frame)

while True:
    ret, frame = cap.read()
    if ret:

        frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

        # 计算特征点
        p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, frame_gray, p0, None, **lk_params)
        # 选取好的特征跟踪点
        # for num in range(len(p1)):
        #     dist = cv2.norm(p1 - p0)
        #     print(dist)
        #     if 2.0 <= dist <= 20.0:  # 合理的特征追踪点
        #         goods_new = p1[st == 1]
        #         goods_old = p0[st == 1]
        goods_new = p1[st == 1]
        goods_old = p0[st == 1]

        # 画出光流轨迹
        for i, (new, old) in enumerate(zip(goods_new, goods_old)):
            a, b = new.ravel()
            c, d = old.ravel()
            mask = cv2.line(mask, (a, b), (c, d), color[i].tolist(), 2)
            frame = cv2.circle(frame, (a, b), 3, color[i].tolist(), -1)

        img = cv2.add(frame, mask)
        cv2.imshow('source', frame)
        cv2.imshow('LK', img)
        k = cv2.waitKey(100) & 0xff
        if k == 27:
            break

        # 更新上一帧的图像和追踪点
        old_gray = frame_gray.copy()
        p0 = goods_new.reshape(-1, 1, 2)
